#include<newGlobal.hpp>

char msgunlock[41] = {
	0xfe, 0x21, 0xae, 0xff, 0xbe, 0x4c, 0, 0, 0x80, 0x3f, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0x90, 0x01, 0x01, 0x01, 0, 0xcd, 0xbb};
char msglock[41] = {
	0xfe, 0x21, 0xb6, 0xff, 0xbe, 0x4c, 0, 0, 0x00, 0x00, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0x90, 0x01, 0x01, 0x01, 0, 0x94, 0x5d};


int main(int argc, char *argv[])
{
	using namespace udpio;
	using namespace asio;
	using namespace std;
	using namespace cv;
	int options[5] = {0};
	for (size_t i = 1; i < argc; i++)
	{
		options[i - 1] = atoi(argv[i]);
	}

	FileStorage fs("data.xml", FileStorage::READ);
	Mat intrinsics, distortion, map1, map2;

	//广角镜头正畸
	fs["camera_matrix"] >> intrinsics;
	fs["distortion_coefficients"] >> distortion;
	fs.release();
	initUndistortRectifyMap(
		intrinsics, distortion, Mat(),
		getOptimalNewCameraMatrix(intrinsics, distortion, Size(640, 480), 1, Size(1000, 750), 0),
		Size(1000, 750),
		CV_16SC2, map1, map2);
	Detector *detector = new Detector("auv.cfg", "auv.tech");

	//打开飞控串口
	asio::serial_port serial(io, serial_port_name);
	if (!serial.is_open())
		cout << "can not open serial files, please check it if this port is opened.\n Auv program is allready running but there is a error holds it back!" << endl;
	mav::mavio mav(serial);
	mavlink_channel_t chan = MAVLINK_COMM_0;
	mav::bind_mavio_to(chan, mav);
	string mav_errmsg;
	//打开摄像头
	int i = -1;
	flag://recheck the camera
	i++;
	cv::VideoCapture cap(i);
	cv::VideoCapture *camera = &cap;
	if (cap.isOpened())
	{
		printmsg("camera 1 is opened");
	}
	else
	{
		goto flag;
	}
	mav.setReceivedFunc(recfunc);

	//mavlink 心跳线程
	asio::steady_timer pixbeater(io);
	pixbeater.async_wait(std::bind(&pixbeatfunc,std::placeholders::_1,&pixbeater,chan));
	//解锁
	serial.async_write_some(buffer(msgunlock, 41), std::bind([]() {}));
	mavlink_msg_request_data_stream_send(chan, 1, 190, 0, 10, 1);
	//检查状态
	asio::steady_timer msg_io(io);
	msg_io.async_wait(std::bind(&check_stream,std::placeholders::_1,&msg_io,chan));
	//运动方程
	asio::steady_timer movekeeptimer(io);
	movekeeptimer.async_wait(std::bind(&movekeepfunc,std::placeholders::_1,&movekeeptimer,chan));

}
